package dhk;

import java.awt.geom.Point2D;
import robocode.Robot;

/**
 * Provides set of commonly used static methods related to robot movement.
 * 
 * @author Dean Kim
 * @version 1.1
 * 
 */
public class MovementUtility {

  /**
   * Compute the value of the battlefields center X point.
   * 
   * @param battleFieldWidth Battlefield width value
   * @return Battlefield center X value
   */
  protected static double computeBattleFieldCenterX(double battleFieldWidth) {
    return (battleFieldWidth / 2);
  }

  /**
   * Compute the value of the battlefields center Y point.
   * 
   * @param battleFieldHeight Battlefield height value
   * @return Battlefield center Y value
   */
  protected static double computeBattleFieldCenterY(double battleFieldHeight) {
    return (battleFieldHeight / 2);
  }

  /**
   * Compute distance between two points.
   * 
   * @param startX Start X point
   * @param startY Start Y point
   * @param endX End X point
   * @param endY End Y point
   * @return Distance value between start and end points
   */
  protected static double computeDistanceToPoint(double startX, double startY, double endX,
      double endY) {

    return Math.hypot(computeAxisDistanceToPoint(startX, endX), computeAxisDistanceToPoint(startY,
        endY));

  }

  /**
   * Compute distance along X or Y axis between two points.
   * 
   * @param startPoint Starting X or Y value
   * @param endPoint Ending X or Y value
   * @return Distance between two points along X or Y axis
   */
  protected static double computeAxisDistanceToPoint(double startPoint, double endPoint) {

    double axisLengthToPoint = 0;

    if (startPoint <= endPoint) {
      axisLengthToPoint = endPoint - startPoint;
    }
    else {
      axisLengthToPoint = startPoint - endPoint;
    }
    return axisLengthToPoint;
  }

  /**
   * Computes rotation in degrees from current true heading to destination true bearing.
   * 
   * @param currentTrueHeading Current true heading
   * @param destinationTrueBearing Destination true heading
   * @return Rotation in degrees
   */
  protected static double computeRotationToBearing(double currentTrueHeading,
      double destinationTrueBearing) {

    double rotation = 0;

    if (destinationTrueBearing >= currentTrueHeading) {
      rotation = destinationTrueBearing - currentTrueHeading;
    }
    else if (destinationTrueBearing < currentTrueHeading) {
      rotation = (destinationTrueBearing + 360) - currentTrueHeading;
    }
    if (rotation >= 180) {
      return (-360 + rotation);
    }
    else if (rotation < 180) {
      return (rotation);
    }

    return 0;
  }

  /**
   * Convert relative bearing to true bearing.
   * 
   * @param currentTrueHeading Current true heading
   * @param relativeBearing Relative bearing
   * @return True bearing value based on the current heading and relative bearing
   */
  protected static double convertRelativeToTrueBearing(double currentTrueHeading,
      double relativeBearing) {

    double newTrueBrg = 0;

    newTrueBrg = currentTrueHeading + relativeBearing;
    if (newTrueBrg < 0) {
      newTrueBrg += 360;
    }
    else if (newTrueBrg >= 360) {
      newTrueBrg -= 360;
    }
    return newTrueBrg;
  }

  /**
   * Compute rotate from from current point and heading to a destination point.
   * 
   * @param currentX Current X value
   * @param currentY Current Y value
   * @param heading Current Heading
   * @param destinationX Destination X value
   * @param desinationY Destination Y value
   * @return Rotation value to destination point
   */
  protected static double computeRotationToPosit(double currentX, double currentY, double heading,
      double destinationX, double desinationY) {

    double lengthTriangleHypotenuse = Math.hypot(
        computeAxisDistanceToPoint(currentX, destinationX), computeAxisDistanceToPoint(currentY,
            desinationY));
    double TriangleAngle = Math.toDegrees(Math.asin(computeAxisDistanceToPoint(currentX,
        destinationX)
        / lengthTriangleHypotenuse));
    double trueBearingToPosit = 0;
    double rotationToPosit = 0;

    if ((currentX >= destinationX) && (currentY >= desinationY)) {
      trueBearingToPosit = 180 + TriangleAngle;
    }

    if ((currentX >= destinationX) && (currentY <= desinationY)) {
      trueBearingToPosit = 360 - TriangleAngle;
    }

    if ((currentX <= destinationX) && (currentY <= desinationY)) {
      trueBearingToPosit = TriangleAngle;
    }

    if ((currentX <= destinationX) && (currentY >= desinationY)) {
      trueBearingToPosit = 180 - TriangleAngle;
    }

    rotationToPosit = (trueBearingToPosit - heading) % 360;

    if (Math.abs(rotationToPosit) <= 180) {
      return (rotationToPosit);

    }
    else {
      if (rotationToPosit >= 180) {
        return (-360 + rotationToPosit);
      }
      else if (rotationToPosit < -180) {
        return (360 + rotationToPosit);
      }
    }

    return rotationToPosit;
  }

  /**
   * Move robot to center.
   * 
   * @param robot Robot instance to move to center
   */
  protected static void moveToCenter(Robot robot) {

    double centerX = MovementUtility.computeBattleFieldCenterX(robot.getBattleFieldWidth());
    double centerY = MovementUtility.computeBattleFieldCenterY(robot.getBattleFieldHeight());

    robot.turnRight(MovementUtility.computeRotationToPosit(robot.getX(), robot.getY(), robot
        .getHeading(), centerX, centerY));
    robot.ahead(MovementUtility
        .computeDistanceToPoint(robot.getX(), robot.getY(), centerX, centerY));
  }

  /**
   * 
   * Move robot to a given position.
   * 
   * @param robot Robot instance to move to position
   * @param currentHeading Robot heading
   * @param currentPosition Current Position
   * @param tacticalHeading Future Heading
   * @param tacticalPosition Future Position
   */
  protected static void moveToPositionAndRotate(Robot robot, double currentHeading,
      Point2D currentPosition, double tacticalHeading, Point2D tacticalPosition) {

    boolean positionReached = false;

    if ((robot.getX() > tacticalPosition.getX() + 20)
        || (robot.getX() < tacticalPosition.getX() - 20)) {
      robot.turnRight(MovementUtility.computeRotationToPosit(currentPosition.getX(),
          currentPosition.getY(), robot.getHeading(), tacticalPosition.getX(), tacticalPosition
              .getY()));
      robot.ahead(MovementUtility.computeDistanceToPoint(robot.getX(), robot.getY(),
          tacticalPosition.getX(), tacticalPosition.getY()));
      if ((robot.getX() < tacticalPosition.getX() + 20)
          && (robot.getX() > tacticalPosition.getX() - 20)) {
        positionReached = true;
      } // end if
    } // end if
    else {
      positionReached = true;
    } // end else

    if ((positionReached) && (robot.getHeading() != tacticalHeading)) {
      robot.turnRight(computeRotationToBearing(robot.getHeading(), tacticalHeading));
    } // end if

  }

}
